#include <crt_util.h>

_queue_t* queue_middle(_queue_t* queue)
{
    _queue_t* middle = NULL;
    _queue_t* next = NULL;

    middle = _queue_head(queue);

    if (middle == _queue_last(queue)) {
        return middle;
    }

    next = _queue_head(queue);

    for (;;) {
        middle = _queue_next(middle);

        next = _queue_next(next);

        if (next == _queue_last(queue)) {
            return middle;
        }

        next = _queue_next(next);

        if (next == _queue_last(queue)) {
            return middle;
        }
    }
}

void queue_sort(_queue_t* queue, int (*cmp)(const _queue_t*, const _queue_t*))
{
    _queue_t *q, *prev, *next = NULL;

    q = _queue_head(queue);

    if (q == _queue_last(queue)) {
        return;
    }

    for (q = _queue_next(q); q != _queue_sentinel(queue); q = next) {
        prev = _queue_prev(q);
        next = _queue_next(q);

        _queue_remove(q);

        do {
            if (cmp(prev, q) <= 0) {
                break;
            }

            prev = _queue_prev(prev);

        } while (prev != _queue_sentinel(queue));

        _queue_insert_after(prev, q);
    }
}
